package endcourseproject;

import lejos.nxt.TouchSensor;

public class Avoid extends Behavior {

	
	
	private TouchSensor _touchSensor = null;
	private DirectionSensor _directionSensor = null;
	
	public Avoid(TouchSensor ts, DirectionSensor ds)
	{
		_touchSensor = ts;
		_directionSensor = ds;
	}
	
	
	private int _direction = 0;
	private float _magnitude = 0;
	
	@Override
	public synchronized int getDirection() {
		
		return _direction;
	}
	
	@Override
	public synchronized float getMagnitude() {
		return _magnitude;
	}

	private synchronized void set(int direction, float magnitude)
	{
		_direction = direction;
		_magnitude = magnitude;
	}
	
	
	private static final float AVOID_MAGNITUDE = 10f;
	
	
	private enum State
	{
		IDLE,
		AVOID_ENABLE,
		AVOID_TIMEOUT
	}
	
	private State _state = State.IDLE;
	
	public void run()
	{
		final int TIMEOUT_CNT = 100;
		int avoid_timeout = 0;
		int direction = 0;
		while(true)
		{		
			boolean pressed = _touchSensor.isPressed();

		
			switch(_state)
			{
			case IDLE:
				set(direction, 0);
				if( pressed )
				{
					_state = State.AVOID_ENABLE;
				}
				else
				{
					Common.sleep(10);
				}
				break;
			case AVOID_ENABLE:
				//Common.beep();
				
				direction = _directionSensor.getDirection() + 180;
				if( direction >=360) direction -=360;
				set(direction, AVOID_MAGNITUDE);
				
				Common.sleep(500); // Give robot time to get away from wall
				
				avoid_timeout = TIMEOUT_CNT;
				_state = State.AVOID_TIMEOUT;
				break;
			case AVOID_TIMEOUT:
				if(!pressed)
				{
					if( avoid_timeout-- > 0)
					{
						Common.sleep(100);
						set(direction, (AVOID_MAGNITUDE * ((float)avoid_timeout/(float)TIMEOUT_CNT)));
					}
					else
					{
						_state = State.IDLE;
					}
				}
				else
				{
					_state = State.AVOID_ENABLE;
				}
				break;
			default:
				_state = State.IDLE;
				break;
			}


		}
	}
	
}
